home *** CD-ROM | disk | FTP | other *** search
/ Celestin Apprentice 2 / Apprentice-Release2.iso / Tools / Languages / RLaB 1.15c / examples / lqrtest.r < prev    next >
Encoding:
Text File  |  1994-09-20  |  1.6 KB  |  79 lines  |  [TEXT/RLAB]
  1.  
  2. printf( "Starting the lqr test...\n" );
  3. rfile ode23
  4.  
  5. lqr = function( a, b, q, r ) 
  6. {
  7.   m = size(a)[1]; n = size(a)[2];
  8.   mb = size(b)[1]; nb = size(b)[2];
  9.   mq = size(q)[1]; nq = size(q)[2];
  10.   
  11.   if ( m != mq || n != nq ) 
  12.   {
  13.     fprintf( "stderr", "A and Q must be the same size.\n" );
  14.     quit
  15.   }
  16.     
  17.   mr = size(r)[1]; nr = size(r)[2];
  18.   if ( mr != nr || nb != mr ) 
  19.   {
  20.     fprintf( "stderr", "B and R must be consistent.\n" );
  21.     quit
  22.   }
  23.     
  24.   nn = zeros( m, nb );
  25.   
  26.   // Start eigenvector decomposition by finding eigenvectors of Hamiltonian:
  27.   
  28.   e = eig( [ a, solve(r',b')'*b'; q, -a' ] );
  29.   v = e.vec; d = e.val;
  30.     
  31.   index = sort( real( d ) ).ind;
  32.   d = real( d[ index ] );
  33.   
  34.   if ( !( d[n] < 0 && d[n+1] > 0 ) ) 
  35.   {
  36.     fprintf( "stderr", "Can't order eigenvalues.\n" );
  37.     quit
  38.   }
  39.     
  40.   chi = v[ 1:n; index[1:n] ];
  41.   lambda = v[ (n+1):(2*n); index[1:n] ];
  42.   s = -real(solve(chi',lambda')');
  43.   k = solve( r, nn'+b'*s );
  44.   
  45.   return << k=k; s=s >>;
  46.   
  47. };
  48.  
  49.  
  50. // Now run a little test problem.
  51.  
  52. k = 1; m = 1; c = .1;
  53. a = [    0,    1,    0,    0; -k/m, -c/m,  k/m,  c/m; 0,    0,    0,    1; k/m,  c/m, -k/m, -c/m ];
  54. b = [ 0; 1/m; 0; 0 ];
  55. qxx = diag( [0, 0, 100, 0] );
  56. ruu = [1];
  57.  
  58. K = lqr( a, b, qxx, ruu ).k;
  59.  
  60. dot = function( t, x ) 
  61. {
  62.   global (a, b, K)
  63.   return (a-b*K)*x + b*K*([1,0,1,0]')
  64. };
  65.  
  66. x = ode( dot, 0, 15, [0,0,0,0]',.01, 1.e-5, 1.e-5);
  67.  
  68. m = maxi( x[;2] );
  69.  
  70. if ( (abs( x[m;2] - 1.195 ) > 0.001)  || ...
  71.      any (abs( x[x.nr;2,4] - 1 ) > 0.001) ) {
  72.      printf( "...failed.\a\n" );
  73. else
  74.      plgrid ();
  75.      ptitle ("LQR Test Results");
  76.      plot (x);
  77.      printf( "...passed.\n" );
  78. }
  79.